#include <math.h>

#include "balance.h"
#include "scope_protocol.h"
#include "mpu6xxx.h"

#define T (0.02f) //采样周期

static rt_thread_t imu_thread = RT_NULL; /* 姿态更新线程 */
static rt_sem_t imu_sem = RT_NULL;       /* imu更新信号量 */

struct vector_3d attitude = {0};

struct balance_control control = {0};
rt_int32_t pwm_left = 0, pwm_right = 0;

static void imu_period_callback()
{
    rt_sem_release(imu_sem);
}

float Q_angle = 0.001; //过程噪声协方差
float Q_gyro = 0.003;  //过程噪声协方差
float R_angle = 0.5;   //测量噪声协方差
float dt = 0.02;
char C_0 = 1;
float Q_bias, Angle_err;
float PCt_0, PCt_1, E;
float K_0, K_1, t_0, t_1;
float Pdot[4] = {0, 0, 0, 0};
float PP[2][2] = {{1, 0}, {0, 1}};

float invSqrt(float x)
{
    float halfx = 0.5f * x;
    float y = x;
    long i = *(long *)&y;
    i = 0x5f3759df - (i >> 1);
    y = *(float *)&i;
    y = y * (1.5f - (halfx * y * y));
    return y;
}

/**
 * 仅计算俯仰角
 */
static void imu_pitch_update(struct mpu6xxx_3axes *accel, struct mpu6xxx_3axes *gyro, struct mpu6xxx_3axes *result)
{
    static float K = 0.1f;
    static float accel_angle = 0, gyro_angle = 0;
    accel_angle = atan2(accel->y, accel->z) * 57.3f;
    gyro->x /= 10;
#if 0
    gyro_angle += gyro->x / 10 * T;
    result->x = K * accel_angle + (1 - K) * gyro_angle;
    gyro_angle = result->x;
#else //卡尔曼滤波
    result->x += (gyro->x - Q_bias) * dt;    //先验估计
    Pdot[0] = Q_angle - PP[0][1] - PP[1][0]; // Pk-先验估计误差协方差的微分

    Pdot[1] = -PP[1][1];
    Pdot[2] = -PP[1][1];
    Pdot[3] = Q_gyro;
    PP[0][0] += Pdot[0] * dt; // Pk-先验估计误差协方差的积分
    PP[0][1] += Pdot[1] * dt; // =先验估计误差协方差
    PP[1][0] += Pdot[2] * dt;
    PP[1][1] += Pdot[3] * dt;

    Angle_err = accel_angle - result->x; // zk-先验估计

    PCt_0 = C_0 * PP[0][0];
    PCt_1 = C_0 * PP[1][0];

    E = R_angle + C_0 * PCt_0;

    K_0 = PCt_0 / E;
    K_1 = PCt_1 / E;

    t_0 = PCt_0;
    t_1 = C_0 * PP[0][1];

    PP[0][0] -= K_0 * t_0; //后验估计误差协方差
    PP[0][1] -= K_0 * t_1;
    PP[1][0] -= K_1 * t_0;
    PP[1][1] -= K_1 * t_1;

    result->x += K_0 * Angle_err; //后验估计
    Q_bias += K_1 * Angle_err;    //后验估计

    // angle_dot = Gyro - Q_bias;    //输出值(后验估计)的微分=角速度
#endif
    protocol_report_data(PROTOCOL_CHANNEL_1, result->x);
    protocol_report_data(PROTOCOL_CHANNEL_2, accel_angle);
}

/**
 * 六轴姿态融合
 */
void MahonyAHRSupdateIMU(struct mpu6xxx_3axes *accel, struct mpu6xxx_3axes *gyro, struct vector_3d *result)
{
    static float Kp = 8.0f, Ki = 0.05f;
    static float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f;
    static float integral_x = 0.0f, integral_y = 0.0f, integral_z = 0.0f;

    float ax, ay, az, gx, gy, gz;
    float recipNorm;
    float halfvx, halfvy, halfvz;
    float halfex, halfey, halfez;
    float qa, qb, qc, qd; //四元数缓存

    //角速度单位转换
    gx = gyro->x / 10.0f / 57.3f;
    gy = gyro->y / 10.0f / 57.3f;
    gz = gyro->z / 10.0f / 57.3f;

    if (!((accel->x == 0.0f) && (accel->y == 0.0f) && (accel->z == 0.0f)))
    {
        // 加速度计规范化
        recipNorm = invSqrt(accel->x * accel->x + accel->y * accel->y + accel->z * accel->z);
        ax = accel->x * recipNorm;
        ay = accel->y * recipNorm;
        az = accel->z * recipNorm;
        // 重力加速度坐标系变换
        halfvx = q1 * q3 - q0 * q2;
        halfvy = q0 * q1 + q2 * q3;
        halfvz = q0 * q0 - 0.5f + q3 * q3;
        // 计算误差
        halfex = (ay * halfvz - az * halfvy);
        halfey = (az * halfvx - ax * halfvz);
        halfez = (ax * halfvy - ay * halfvx);

        // Compute and apply integral feedback if enabled
        if (Ki > 0.0f)
        {
            integral_x += Ki * halfex * T; // integral error scaled by Ki
            integral_y += Ki * halfey * T;
            integral_z += Ki * halfez * T;
        }
        else
        {
            integral_x = 0.0f;
            integral_y = 0.0f;
            integral_z = 0.0f;
        }

        // Apply proportional feedback
        gx += Kp * halfex + integral_x;
        gy += Kp * halfey + integral_y;
        gz += Kp * halfez + integral_z;
    }

    // 一阶龙格库塔法
    qa = q0;
    qb = q1;
    qc = q2;
    qd = q3;
    q0 += (-qb * gx - qc * gy - qd * gz) * (0.5f * T);
    q1 += (qa * gx + qc * gz - qd * gy) * (0.5f * T);
    q2 += (qa * gy - qb * gz + qd * gx) * (0.5f * T);
    q3 += (qa * gz + qb * gy - qc * gx) * (0.5f * T);

    // 四元数规范化
    recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
    q0 *= recipNorm;
    q1 *= recipNorm;

    q2 *= recipNorm;
    q3 *= recipNorm;

    result->x = asinf(-2.0f * q1 * q3 + 2 * q0 * q2) * 57.3;                                  // pitch
    result->y = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2 * q2 + 1) * 57.3;      // roll
    result->z = atan2(2 * (q1 * q2 + q0 * q3), q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3) * 57.3; // yaw
    // protocol_report_data(PROTOCOL_CHANNEL_1, result->x);
    // protocol_report_data(PROTOCOL_CHANNEL_2, result->y);
    // protocol_report_data(PROTOCOL_CHANNEL_3, result->z);
}

static void imu_update(struct mpu6xxx_3axes *accel, struct mpu6xxx_3axes *gyro, struct mpu6xxx_3axes *result)
{
    static float Kp = 100.0f, Ki = 0.002f;               //比例增益支配率(常量) 积分增益支配率
    static float g_q0 = 1, g_q1 = 0, g_q2 = 0, g_q3 = 0; // Quaternion
    static float qa = 1, qb = 0, qc = 0, qd = 0;         // Quaternion
    static float g_exInt = 0, g_eyInt = 0, g_ezInt = 0;

    float norm; //模
    float vx, vy, vz;
    float ex, ey, ez;
    //角速度单位转换
    gyro->x = gyro->x / 10.0f / 57.3f;
    gyro->y = gyro->y / 10.0f / 57.3f;
    gyro->z = gyro->z / 10.0f / 57.3f;
    //归一化
    norm = sqrt(accel->x * accel->x + accel->y * accel->y + accel->z * accel->z); //取模
    accel->x = accel->x / norm;
    accel->y = accel->y / norm;
    accel->z = accel->z / norm;

    //估计方向的重力
    vx = 2 * (g_q1 * g_q3 - g_q0 * g_q2);
    vy = 2 * (g_q0 * g_q1 + g_q2 * g_q3);
    // vz = g_q0 * g_q0 - g_q1 * g_q1 - g_q2 * g_q2 + g_q3 * g_q3;
    vz = 2 * (g_q0 * g_q0 + g_q3 * g_q3) - 1;

    //错误的领域和方向传感器测量参考方向几件的交叉乘积的总和
    ex = (accel->y * vz - accel->z * vy);
    ey = (accel->z * vx - accel->x * vz);
    ez = (accel->x * vy - accel->y * vx);

    //积分误差比例积分增益
    g_exInt += ex * Ki * T;
    g_eyInt += ey * Ki * T;
    g_ezInt += ez * Ki * T;

    //调整后的陀螺仪测量
    gyro->x += Kp * ex + g_exInt;
    gyro->y += Kp * ey + g_eyInt;
    gyro->z += Kp * ez + g_ezInt;

    qa = g_q0;
    qb = g_q1;
    qc = g_q2;
    qd = g_q3;
    //整合四元数率和正常化
    g_q0 += (-qb * gyro->x - qc * gyro->y - qd * gyro->z) * T / 2;
    g_q1 += (qa * gyro->x + qc * gyro->z - qd * gyro->y) * T / 2;
    g_q2 += (qa * gyro->y - qb * gyro->z + qd * gyro->x) * T / 2;
    g_q3 += (qa * gyro->z + qb * gyro->y - qc * gyro->x) * T / 2;

    //规范化四元数
    norm = sqrt(g_q0 * g_q0 + g_q1 * g_q1 + g_q2 * g_q2 + g_q3 * g_q3);
    g_q0 = g_q0 / norm;
    g_q1 = g_q1 / norm;
    g_q2 = g_q2 / norm;
    g_q3 = g_q3 / norm;

    result->x = asin(-2 * g_q1 * g_q3 + 2 * g_q0 * g_q2) * 57.3;                                                      // pitch
    result->y = atan2(2 * g_q2 * g_q3 + 2 * g_q0 * g_q1, -2 * g_q1 * g_q1 - 2 * g_q2 * g_q2 + 1) * 57.3;              // roll
    result->z = atan2(2 * (g_q1 * g_q2 + g_q0 * g_q3), g_q0 * g_q0 + g_q1 * g_q1 - g_q2 * g_q2 - g_q3 * g_q3) * 57.3; // yaw
    protocol_report_data(PROTOCOL_CHANNEL_1, result->x);
    protocol_report_data(PROTOCOL_CHANNEL_2, result->y);
    protocol_report_data(PROTOCOL_CHANNEL_3, result->z);
}

#define MECHINE_BALANCE_ANGLE (0.0f) //机械平衡角度
/**
 * 直立环PD调节
 * gyro：角速度，角度的微分
 */
static float balance(float angle, float gyro)
{
    // static float balanceKp = 400.0f, balanceKd = 0.15;
    float bias = angle - MECHINE_BALANCE_ANGLE;           //角度偏差
    return info.balanceKp * bias + gyro * info.balanceKd; //===计算平衡控制的电机PWM  PD控制   kp是P系数 kd是D系数;
}

/**
 * 速度环PI调节
 * 左右编码器输入，速度=编码器(位移)/时间，等周期所以用编码器值代替速度
 */
static float velocity(int32_t encoder_left, int32_t encoder_right)
{
    // static float velocityKp = 80, velocityKi = 0.4f;
    static float encoder, encoder_new, movement, encoder_integral;
    static float speed, gear; //速度系数，用于参与挡位计算
    //=============遥控前进后退部分=======================//
    //前进
    if (control.forward)
    {
        movement = speed * gear;
    }
    else if (control.backward)
    {
        movement = -speed * gear;
    }
    else
    {
        movement = 0;
    }

    encoder_new = (encoder_left + encoder_right) - 0; //获取最新速度偏差==测量速度（左右编码器之和）-目标速度（此处为零）
    encoder = encoder * 0.8 + encoder_new * 0.2;      //一阶低通滤波器

    encoder_integral += encoder;                    //===积分出位移 积分时间：10ms
    encoder_integral = encoder_integral - movement; //===接收遥控器数据，控制前进后退

    //积分限幅
    if (encoder_integral > 10000)
        encoder_integral = 10000;
    if (encoder_integral < -10000)
        encoder_integral = -10000;

    return encoder * info.velocityKp + encoder_integral * info.velocityKi;
}

/**
 * 转向环PD调节
 */
static int turn(int encoder_left, int encoder_right, float gyro) //转向控制
{
    static float trunKp = 42, trunKd = 0;
    static float Turn_Target, Encoder_temp, Turn_Convert = 0.9, Turn_Count;
    float Turn_Amplitude = 88 * control.speed;
    //这一部分主要是根据旋转前的速度调整速度的起始速度，增加小车的适应性
    // if (1 == control.left || 1 == control.right)
    // {
    //     if (++Turn_Count == 1)
    //         Encoder_temp = myabs(encoder_left + encoder_right);
    //     Turn_Convert = 50 / Encoder_temp;
    //     if (Turn_Convert < 0.6) //转向速度限幅
    //         Turn_Convert = 0.6;
    //     if (Turn_Convert > 3)
    //         Turn_Convert = 3;
    // }
    // else
    // {
    //     Turn_Convert = 0.9;
    //     Turn_Count = 0;
    //     Encoder_temp = 0;
    // }

    if (1 == control.left)
        Turn_Target -= Turn_Convert;
    else if (1 == control.right)
        Turn_Target += Turn_Convert;
    else
        Turn_Target = 0;

    if (Turn_Target > Turn_Amplitude) //转向速度限幅
        Turn_Target = Turn_Amplitude;
    if (Turn_Target < -Turn_Amplitude)
        Turn_Target = -Turn_Amplitude;

    //转向的时候取消陀螺仪的纠正
    if (control.forward == 1 || control.backward == 1)
        trunKd = 0.5;
    else
        trunKd = 0;

    return -Turn_Target * trunKp - gyro * trunKd; //===结合Z轴陀螺仪进行PD控制
}

static void imu_entry()
{
    static rt_err_t ret;

    struct mpu6xxx_device *dev;
    struct mpu6xxx_3axes accel, gyro;
    static rt_timer_t timer1;
    static int32_t encoderLeft, encoderRight;
    rt_int32_t pwm_balance, pwm_velocity, pwm_turn;

    /* Initialize mpu6xxx, The parameter is RT_NULL, means auto probing for i2c*/
    dev = mpu6xxx_init("i2c1", RT_NULL);
    if (dev == RT_NULL)
    {
        rt_kprintf("mpu6xxx init failed\n");
        return;
    }
    imu_sem = rt_sem_create("imu_sem", 1, RT_IPC_FLAG_PRIO);

    /* 创建定时器 1  周期定时器 */
    timer1 = rt_timer_create("timer1", imu_period_callback,
                             RT_NULL, T * 1000,
                             RT_TIMER_FLAG_PERIODIC);
    rt_timer_start(timer1);

    while (1)
    {
        ret = rt_sem_take(imu_sem, RT_WAITING_FOREVER);

        mpu6xxx_get_accel(dev, &accel);
        mpu6xxx_get_gyro(dev, &gyro);
        // rt_kprintf("accel.x = %3d, accel.y = %3d, accel.z = %3d ", accel.x, accel.y, accel.z);
        // rt_kprintf("gyro.x = %3d gyro.y = %3d, gyro.z = %3d\n", gyro.x, gyro.y, gyro.z);
#if 0
        imu_pitch_update(&accel, &gyro, &result);
#else
        // imu_update(&accel, &gyro, &result);
        MahonyAHRSupdateIMU(&accel, &gyro, &attitude);
#endif
        //直立环PID
        pwm_balance = balance(attitude.x, gyro.y) * 1000.0f;
        encoderLeft = get_motor_encoder(MOTOR_INDEX_1);
        encoderRight = get_motor_encoder(MOTOR_INDEX_2);
        pwm_velocity = velocity(encoderLeft, encoderRight) * 1000.0f;
        pwm_left = pwm_balance + pwm_velocity;
        pwm_right = pwm_balance + pwm_velocity;
        //限幅
        if (pwm_left < -10000000)
            pwm_left = -10000000;
        if (pwm_left > 10000000)
            pwm_left = 10000000;
        if (pwm_right < -10000000)
            pwm_right = -10000000;
        if (pwm_right > 10000000)
            pwm_right = 10000000;
        // protocol_report_data(PROTOCOL_CHANNEL_1, attitude.x);
        // protocol_report_data(PROTOCOL_CHANNEL_2, gyro.y);
        // protocol_report_data(PROTOCOL_CHANNEL_3, pwm_left);

        // protocol_report_data(PROTOCOL_CHANNEL_2, encoderRight);
        /**
         * TODO:此处需要做电源补偿，同样占空比在不同电压下输出不同，需要做补偿
         */
        if (info.power > 11.1f) //欠压保护
        {
            set_motor_pwm(MOTOR_INDEX_1, pwm_left);
            set_motor_pwm(MOTOR_INDEX_2, pwm_right);
        }
    }

    mpu6xxx_deinit(dev);
}

static int mpu6050_init(const char *name)
{
    imu_thread = rt_thread_create("imu_thread",
                                  imu_entry,
                                  RT_NULL,
                                  THREAD_STACK_SIZE,
                                  THREAD_PRIORITY,
                                  THREAD_TIMESLICE);
    if (imu_thread != RT_NULL)
    {
        rt_thread_startup(imu_thread);
    }
    return 0;
}
INIT_APP_EXPORT(mpu6050_init);
